'''
单目视觉单点标定，用全站仪生成控制点，生成相机内参和外参
2025-06-16
作者：黄喆
'''
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import cv2
import time

# 设置numpy打印选项，取消科学计数法
np.set_printoptions(suppress=True, precision=6)

# 相机物理参数
FOCAL_LENGTH = 150.0           # 焦距（mm）
PIXEL_SIZE = 0.0029        # 像元尺寸（mm/pixel）

# 计算像素焦距
FOCAL_LENGTH_X = FOCAL_LENGTH / PIXEL_SIZE  # 焦距x（像素）
FOCAL_LENGTH_Y = FOCAL_LENGTH / PIXEL_SIZE  # 焦距y（像素）

# 其他相机参数
PRINCIPAL_POINT_X = 960.0      # 主点x坐标（像素）
PRINCIPAL_POINT_Y = 540.0      # 主点y坐标（像素）
IMAGE_WIDTH = 1920            # 图像宽度（像素）
IMAGE_HEIGHT = 1080           # 图像高度（像素）

# 误差参数
IMAGE_CENTROID_ERROR = 0.5    # 图像质心误差（像素）
TOTAL_STATION_PIXEL_ERROR = 1  # 全站仪像素误差（像素）

# 标定参数
NUM_CALIBRATION_POINTS = 50  # 标定点数量
NEAR_DISTANCE = 10000       # 3D点最近距离（毫米）
FAR_DISTANCE = 150000       # 3D点最远距离（毫米）

def generate_camera_intrinsics(kx,ky,cx,cy): 
    """
    生成相机内参
    返回：
    - K: 3x3相机内参矩阵
    - dist_coeffs: 畸变系数
    """
    K = np.array([
        [kx, 0, cx],
        [0, ky, cy],
        [0, 0, 1]
    ])
    return K

def generate_camera_extrinsics():
    """
    生成相机外参数，确保：
    1. 全站仪和相机的各轴姿态角相差不超过10°
    2. 距离不超过5m
    返回：
    - R_cw: 3x3旋转矩阵
    - T_cw: 3x1平移向量
    """
    # 生成随机欧拉角（以度为单位）
    roll = np.random.uniform(-10, 10)  # 绕X轴旋转
    pitch = np.random.uniform(-10, 10)  # 绕Y轴旋转
    yaw = np.random.uniform(-10, 10)   # 绕Z轴旋转
    
    # 转换为弧度
    roll_rad = np.radians(roll)
    pitch_rad = np.radians(pitch)
    yaw_rad = np.radians(yaw)
    
    # 计算旋转矩阵
    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(roll_rad), -np.sin(roll_rad)],
        [0, np.sin(roll_rad), np.cos(roll_rad)]
    ])
    
    Ry = np.array([
        [np.cos(pitch_rad), 0, np.sin(pitch_rad)],
        [0, 1, 0],
        [-np.sin(pitch_rad), 0, np.cos(pitch_rad)]
    ])
    
    Rz = np.array([
        [np.cos(yaw_rad), -np.sin(yaw_rad), 0],
        [np.sin(yaw_rad), np.cos(yaw_rad), 0],
        [0, 0, 1]
    ])
    
    # 组合旋转矩阵
    R_cw = Rz @ Ry @ Rx
    
    # 生成随机平移向量（距离不超过5000mm）
    distance = np.random.uniform(0, 5000)
    # 生成随机方向（单位向量）
    direction = np.random.randn(3)
    direction = direction / np.linalg.norm(direction)
    # 计算平移向量
    T_cw = direction * distance
    
    return R_cw, T_cw

def generate_2d_points(num_points=10):
    """生成随机2D图像点"""
    # 在图像范围内生成随机点
    points = np.random.uniform(
        low=[0, 0],
        high=[IMAGE_WIDTH, IMAGE_HEIGHT],
        size=(num_points, 2)
    )
    print(points)
    return points
def generate_3d_points(num_points=10,img_points=None,near_distance=100,far_distance=1000):
    """生成随机3D相机点"""
    """
    根据2D图像点生成对应的3D点
    
    参数:
        num_points: 生成点的数量
        img_points: 2D图像点坐标数组
        near_distance: 最近距离(mm)
        far_distance: 最远距离(mm)
        
    返回:
        points_3d: 生成的3D点坐标数组
    """
    # 检查输入参数
    if img_points is None or len(img_points) == 0:
        raise ValueError("必须提供2D图像点坐标")
    
    if len(img_points) < num_points:
        raise ValueError("2D点的数量必须大于等于要生成的3D点数量")
        
    # 随机生成深度值
    depths = np.random.uniform(near_distance, far_distance, num_points)
    
    # 初始化3D点数组
    points_3d = np.zeros((num_points, 3))
    
    for i in range(num_points):
        # 将图像坐标转换为归一化坐标
        x = (img_points[i,0] - PRINCIPAL_POINT_X) / FOCAL_LENGTH_X
        y = (img_points[i,1] - PRINCIPAL_POINT_Y) / FOCAL_LENGTH_Y
        
        # 根据深度值计算3D坐标
        points_3d[i,0] = x * depths[i]  # X = Z * (u-u0)/fx
        points_3d[i,1] = y * depths[i]  # Y = Z * (v-v0)/fy 
        points_3d[i,2] = depths[i]      # Z = depth
        
    print(f"\n生成的3D点坐标:\n{points_3d}")
    return points_3d
def transform_3d_points(points_3d,R_cw,T_cw):
    """将相机坐标系3D点转换到全站仪坐标系"""
    points_3d_world = np.dot(R_cw,points_3d.T).T + T_cw
    return points_3d_world
def plot_points(points_2d, points_3d):
    """在matplotlib中显示2D点和3D点"""
    # 创建子图
    fig = plt.figure(figsize=(20, 8))
    
    # 2D点显示
    ax_2d = fig.add_subplot(121)
    # 绘制图像边界
    ax_2d.plot([0, IMAGE_WIDTH, IMAGE_WIDTH, 0, 0], 
             [0, 0, IMAGE_HEIGHT, IMAGE_HEIGHT, 0], 
             'k-', linewidth=1)
    
    # 绘制主点
    ax_2d.plot(PRINCIPAL_POINT_X, PRINCIPAL_POINT_Y, 'r+', markersize=10, label='主点')
    
    # 绘制随机点
    ax_2d.scatter(points_2d[:, 0], points_2d[:, 1], c='blue', s=50, label='随机点')
    
    # 添加点编号
    for i, point in enumerate(points_2d):
        ax_2d.text(point[0], point[1], f'P{i+1}', 
                color='blue', fontsize=10, fontweight='bold',
                ha='right', va='bottom')
        # 添加坐标值
        ax_2d.text(point[0], point[1] + 15, 
                f'({point[0]:.1f}, {point[1]:.1f})',
                color='black', fontsize=8,
                ha='right', va='bottom')
    
    # 设置2D图属性
    ax_2d.set_title('2D image points')
    ax_2d.set_xlabel('X (pixels)')
    ax_2d.set_ylabel('Y (pixels)')
    ax_2d.grid(True)
    ax_2d.axis('equal')
    ax_2d.legend()
    
    # 设置2D图坐标轴范围
    ax_2d.set_xlim(-100, IMAGE_WIDTH + 100)
    ax_2d.set_ylim(IMAGE_HEIGHT + 100, -100)  # 注意：图像坐标系y轴向下
    
    # 3D点显示
    ax_3d = fig.add_subplot(122, projection='3d')
    
    # 绘制3D点
    ax_3d.scatter(points_3d[:, 0], points_3d[:, 1], points_3d[:, 2], 
                 c='red', s=50, label='3D点')
    
    # 添加点编号和坐标
    for i, point in enumerate(points_3d):
        ax_3d.text(point[0], point[1], point[2], f'P{i+1}',
                  color='red', fontsize=10, fontweight='bold')
        # 添加坐标值
        ax_3d.text(point[0], point[1], point[2] + 1000, 
                  f'({point[0]:.1f}, {point[1]:.1f}, {point[2]:.1f})',
                  color='black', fontsize=8)
    
    # 设置3D图属性
    ax_3d.set_title('3D camera points')
    ax_3d.set_xlabel('X (mm)')
    ax_3d.set_ylabel('Y (mm)')
    ax_3d.set_zlabel('Z (mm)')
    ax_3d.grid(True)
    ax_3d.legend()
    
    # 设置3D图视角
    ax_3d.view_init(elev=20, azim=45)
    
    # 调整子图之间的间距
    plt.tight_layout()
    
    # 显示图像
    plt.show()
def calculate_errors(true_camera_matrix, cal_camera_matrix, true_dist_coeffs, cal_dist_coeffs, true_R_cw, cal_R_cw, true_T_cw, cal_T_cw):
    """
    计算标定结果的误差
    参数：
    - true_camera_matrix: 真实的相机内参矩阵
    - cal_camera_matrix: 标定得到的相机内参矩阵
    - true_dist_coeffs: 真实的畸变系数
    - cal_dist_coeffs: 标定得到的畸变系数
    - true_R_cw: 真实的旋转矩阵
    - cal_R_cw: 标定得到的旋转矩阵
    - true_T_cw: 真实的平移向量
    - cal_T_cw: 标定得到的平移向量
    返回：
    - K_error: 相机内参矩阵的误差
    - dist_coeffs_error: 畸变系数的误差
    - Rcw_error: 旋转矩阵的误差
    - Tcw_error: 平移向量的误差
    """
    # 计算相机内参矩阵的误差
    K_error = np.abs(cal_camera_matrix - true_camera_matrix)
    
    # 计算畸变系数的误差
    dist_coeffs_error = np.abs(cal_dist_coeffs - true_dist_coeffs)
    
    # 计算旋转矩阵的误差
    Rcw_error = np.abs(cal_R_cw - true_R_cw)
    
    # 计算平移向量的误差（确保向量维度一致）
    true_T_cw = np.array(true_T_cw).flatten()
    cal_T_cw = np.array(cal_T_cw).flatten()
    Tcw_error = np.abs(cal_T_cw - true_T_cw)
    
    return K_error, dist_coeffs_error, Rcw_error, Tcw_error
def calculate_camera_matrix(points_3d_total_station, points_2d):
    """
    使用CALIB_USE_INTRINSIC_GUESS方法进行相机标定
    参数：
    - points_3d_total_station: 全站仪坐标系下的3D点坐标 (N, 3)
    - points_2d: 图像坐标系下的2D点坐标 (N, 2)
    返回：
    - camera_matrix: 相机内参矩阵
    - dist_coeffs: 畸变系数
    - rvec: 旋转向量
    - tvec: 平移向量
    """
    # 准备数据格式
    object_points = points_3d_total_station.astype(np.float32)
    image_points = points_2d.astype(np.float32)
    
    # 图像尺寸
    image_size = (IMAGE_WIDTH, IMAGE_HEIGHT)
    
    # 初始相机矩阵估计（添加误差）
    fx_error = np.random.uniform(-50, 50)  # 修改为±50像素
    fy_error = np.random.uniform(-50, 50)  # 修改为±50像素
    cx_error = np.random.uniform(-30, 30)
    cy_error = np.random.uniform(-30, 30)
    
    initial_camera_matrix = np.array([
        [FOCAL_LENGTH_X + fx_error, 0, PRINCIPAL_POINT_X + cx_error],
        [0, FOCAL_LENGTH_Y + fy_error, PRINCIPAL_POINT_Y + cy_error],
        [0, 0, 1]
    ], dtype=np.float32)
    
    print(f"\n初始相机内参矩阵（带误差）：")
    print(initial_camera_matrix)
    
    # 使用OpenCV进行相机标定
    ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
        [object_points],
        [image_points],
        image_size,
        initial_camera_matrix,
        None,
        #flags=cv2.CALIB_USE_INTRINSIC_GUESS + 
        # cv2.CALIB_FIX_PRINCIPAL_POINT + 
        # cv2.CALIB_FIX_ASPECT_RATIO + 
        # cv2.CALIB_ZERO_TANGENT_DIST
        flags=cv2.CALIB_USE_INTRINSIC_GUESS
    )
    
    return camera_matrix, dist_coeffs, rvecs[0], tvecs[0]

def convert_vectors_to_matrices(rvec, tvec):
    """
    将旋转向量和平移向量转换为旋转矩阵和平移矩阵
    参数：
    - rvec: 旋转向量 (3,1)
    - tvec: 平移向量 (3,1)
    返回：
    - R: 3x3旋转矩阵
    - T: 3x1平移矩阵
    """
    # 使用Rodrigues公式将旋转向量转换为旋转矩阵
    R, _ = cv2.Rodrigues(rvec)
    # 平移向量已经是3x1的矩阵形式
    T = tvec.reshape(3, 1)
    return R, T

def convert_camera_to_world(rvec, tvec):
    """
    将相机标定得到的全站仪相对于相机的变换转换为相机相对于全站仪的变换
    参数：
    - rvec: 全站仪相对于相机的旋转向量
    - tvec: 全站仪相对于相机的平移向量
    返回：
    - R_cw: 相机相对于全站仪的旋转矩阵
    - T_cw: 相机相对于全站仪的平移向量
    """
    # 将旋转向量转换为旋转矩阵
    R_wc, _ = cv2.Rodrigues(rvec)
    # 计算相机相对于全站仪的旋转矩阵（转置）
    R_cw = R_wc.T
    # 计算相机相对于全站仪的平移向量
    T_cw = -R_cw @ tvec
    return R_cw, T_cw

def add_gaussian_noise(points, std_dev):
    """
    添加高斯噪声
    参数：
    - points: 输入点坐标
    - std_dev: 标准差
    返回：
    - 添加噪声后的点坐标
    """
    noise = np.random.normal(0, std_dev, points.shape)
    return points + noise

def format_calibration_results(method, results, true_values):
    """
    格式化标定结果
    """
    output = []
    output.append(f"标定方法: {method}")
    output.append("=" * 50)
    
    # 畸变系数
    output.append("\n畸变系数:")
    output.append("-" * 30)
    output.append("参数\t\t真实值\t\t标定值\t\t误差")
    output.append("-" * 50)
    
    # 确保数据是numpy数组并展平
    dist_coeffs_true = np.array(true_values['dist_coeffs']).flatten()
    dist_coeffs_cal = np.array(results['dist_coeffs']).flatten()
    dist_coeffs_error = np.array(results['dist_coeffs_error']).flatten()
    
    for i in range(len(dist_coeffs_true)):
        output.append(f"k{i+1}\t\t{dist_coeffs_true[i]:.6f}\t\t{dist_coeffs_cal[i]:.6f}\t\t{dist_coeffs_error[i]:.6f}")
    
    # 旋转矩阵
    output.append("\n旋转矩阵:")
    output.append("-" * 30)
    output.append("参数\t\t真实值\t\t标定值\t\t误差")
    output.append("-" * 50)
    
    # 确保数据是numpy数组
    R_cw_true = np.array(true_values['R_cw'])
    R_cw_cal = np.array(results['R_cw'])
    Rcw_error = np.array(results['Rcw_error'])
    
    for i in range(3):
        for j in range(3):
            output.append(f"R[{i},{j}]\t\t{R_cw_true[i][j]:.6f}\t\t{R_cw_cal[i][j]:.6f}\t\t{Rcw_error[i][j]:.6f}")
    
    # 相机内参
    output.append("\n相机内参:")
    output.append("-" * 30)
    output.append("参数\t\t真实值\t\t标定值\t\t误差")
    output.append("-" * 50)
    
    # 确保数据是numpy数组
    camera_matrix_true = np.array(true_values['camera_matrix'])
    camera_matrix_cal = np.array(results['camera_matrix'])
    K_error = np.array(results['K_error'])
    
    # fx
    output.append(f"fx\t\t{camera_matrix_true[0][0]:.2f}\t\t{camera_matrix_cal[0][0]:.2f}\t\t{K_error[0][0]:.2f}")
    
    # fy
    output.append(f"fy\t\t{camera_matrix_true[1][1]:.2f}\t\t{camera_matrix_cal[1][1]:.2f}\t\t{K_error[1][1]:.2f}")
    
    # cx
    output.append(f"cx\t\t{camera_matrix_true[0][2]:.2f}\t\t{camera_matrix_cal[0][2]:.2f}\t\t{K_error[0][2]:.2f}")
    
    # cy
    output.append(f"cy\t\t{camera_matrix_true[1][2]:.2f}\t\t{camera_matrix_cal[1][2]:.2f}\t\t{K_error[1][2]:.2f}")
    
    # 平移向量
    output.append("\n平移向量:")
    output.append("-" * 30)
    output.append("参数\t\t真实值\t\t标定值\t\t误差")
    output.append("-" * 50)
    
    # 确保数据是numpy数组并展平
    T_cw_true = np.array(true_values['T_cw']).flatten()
    T_cw_cal = np.array(results['T_cw']).flatten()
    Tcw_error = np.array(results['Tcw_error']).flatten()
    
    for i in range(3):
        output.append(f"T[{i}]\t\t{T_cw_true[i]:.6f}\t\t{T_cw_cal[i]:.6f}\t\t{Tcw_error[i]:.6f}")
    
    return "\n".join(output)

def main():
    # 生成随机点
    points = generate_2d_points(NUM_CALIBRATION_POINTS)
    
    # 生成相机外参数
    R_cw_true, T_cw_true = generate_camera_extrinsics()
    print("\n真实的相机外参数：")
    print("旋转矩阵 R_cw:")
    print(R_cw_true)
    print("\n平移向量 T_cw:")
    print(T_cw_true)
    
    # 显示点
    points_3d_camera = generate_3d_points(NUM_CALIBRATION_POINTS, points, near_distance=NEAR_DISTANCE, far_distance=FAR_DISTANCE)
    points_3d_total_station = transform_3d_points(points_3d_camera, R_cw_true, T_cw_true)
    
    # 添加随机误差
    points_with_noise = add_gaussian_noise(points, IMAGE_CENTROID_ERROR)
    points_3d_total_station_with_noise = add_gaussian_noise(points_3d_total_station, TOTAL_STATION_PIXEL_ERROR)
    
    # 生成真实的相机内参
    true_camera_matrix = generate_camera_intrinsics(FOCAL_LENGTH_X, FOCAL_LENGTH_Y, PRINCIPAL_POINT_X, PRINCIPAL_POINT_Y)
    true_dist_coeffs = np.zeros(5)
    
    # 存储真实值
    true_values = {
        'camera_matrix': np.array(true_camera_matrix),
        'dist_coeffs': np.array(true_dist_coeffs),
        'R_cw': np.array(R_cw_true),
        'T_cw': np.array(T_cw_true)
    }
    
    print("\n开始相机标定：")
    try:
        camera_matrix, dist_coeffs, rvec, tvec = calculate_camera_matrix(
            points_3d_total_station_with_noise, 
            points_with_noise
        )
        
        # 将全站仪相对于相机的变换转换为相机相对于全站仪的变换
        R_cw_result, T_cw_result = convert_camera_to_world(rvec, tvec)
        
        # 确保所有数据都是numpy数组
        camera_matrix = np.array(camera_matrix)
        dist_coeffs = np.array(dist_coeffs)
        R_cw_result = np.array(R_cw_result)
        T_cw_result = np.array(T_cw_result)
        
        # 计算误差
        K_error, dist_coeffs_error, Rcw_error, Tcw_error = calculate_errors(
            true_values['camera_matrix'],
            camera_matrix,
            true_values['dist_coeffs'],
            dist_coeffs,
            true_values['R_cw'],
            R_cw_result,
            true_values['T_cw'],
            T_cw_result
        )
        
        # 存储结果
        results = {
            'camera_matrix': camera_matrix,
            'dist_coeffs': dist_coeffs,
            'R_cw': R_cw_result,
            'T_cw': T_cw_result,
            'K_error': K_error,
            'dist_coeffs_error': dist_coeffs_error,
            'Rcw_error': Rcw_error,
            'Tcw_error': Tcw_error
        }
        
        # 显示详细结果
        print("\n" + "=" * 80)
        print(format_calibration_results("CALIB_USE_INTRINSIC_GUESS", results, true_values))
        print("=" * 80)
        
        return results, true_values
        
    except Exception as e:
        print(f"标定失败：{str(e)}")
        import traceback
        print(traceback.format_exc())
        return None, None

if __name__ == "__main__":
    # 存储所有迭代的结果
    all_fx_errors = []
    all_fy_errors = []
    all_cx_errors = []
    all_cy_errors = []
    
    # 存储最后一次成功标定的结果
    last_successful_results = None
    last_true_values = None
    
    # 运行多次标定
    for i in range(50):
        results, true_values = main()
        if results is not None:  # 确保标定成功
            # 收集误差数据
            K_error = results['K_error']
            all_fx_errors.append(K_error[0,0])
            all_fy_errors.append(K_error[1,1])
            all_cx_errors.append(K_error[0,2])
            all_cy_errors.append(K_error[1,2])
            # 保存最后一次成功的结果
            last_successful_results = results
            last_true_values = true_values
    
    if len(all_fx_errors) > 0:  # 确保有成功的数据
        # 创建四个子图
        fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(12, 8))

        # 创建总标题
        title_info = f'Focal Length: {FOCAL_LENGTH}mm, Pixel Size: {PIXEL_SIZE}mm, Calibration Points: {NUM_CALIBRATION_POINTS}'
        fig.suptitle(title_info, fontsize=12, y=0.95)
        
        # 创建迭代次数数组
        iterations = np.arange(len(all_fx_errors))
        
        # 绘制fx误差
        ax1.plot(iterations, all_fx_errors, 'bo-', label='Error')
        ax1.axhline(y=np.mean(all_fx_errors), color='r', linestyle='--', label=f'Mean: {np.mean(all_fx_errors):.2f}')
        ax1.set_title('fx Error')
        ax1.set_xlabel('Iteration')
        ax1.set_ylabel('Error (pixels)')
        ax1.grid(True)
        ax1.legend()
        
        # 绘制fy误差
        ax2.plot(iterations, all_fy_errors, 'ro-', label='Error')
        ax2.axhline(y=np.mean(all_fy_errors), color='r', linestyle='--', label=f'Mean: {np.mean(all_fy_errors):.2f}')
        ax2.set_title('fy Error')
        ax2.set_xlabel('Iteration')
        ax2.set_ylabel('Error (pixels)')
        ax2.grid(True)
        ax2.legend()
        
        # 绘制cx误差
        ax3.plot(iterations, all_cx_errors, 'go-', label='Error')
        ax3.axhline(y=np.mean(all_cx_errors), color='r', linestyle='--', label=f'Mean: {np.mean(all_cx_errors):.2f}')
        ax3.set_title('cx Error')
        ax3.set_xlabel('Iteration')
        ax3.set_ylabel('Error (pixels)')
        ax3.grid(True)
        ax3.legend()
        
        # 绘制cy误差
        ax4.plot(iterations, all_cy_errors, 'mo-', label='Error')
        ax4.axhline(y=np.mean(all_cy_errors), color='r', linestyle='--', label=f'Mean: {np.mean(all_cy_errors):.2f}')
        ax4.set_title('cy Error')
        ax4.set_xlabel('Iteration')
        ax4.set_ylabel('Error (pixels)')
        ax4.grid(True)
        ax4.legend()
        
        plt.tight_layout()
        plt.show()
        
        # 打印统计信息
        print("\n误差统计信息：")
        print("=" * 50)
        print(f"fx误差 - 平均值: {np.mean(all_fx_errors):.2f}, 标准差: {np.std(all_fx_errors):.2f}")
        print(f"fy误差 - 平均值: {np.mean(all_fy_errors):.2f}, 标准差: {np.std(all_fy_errors):.2f}")
        print(f"cx误差 - 平均值: {np.mean(all_cx_errors):.2f}, 标准差: {np.std(all_cx_errors):.2f}")
        print(f"cy误差 - 平均值: {np.mean(all_cy_errors):.2f}, 标准差: {np.std(all_cy_errors):.2f}")

        # 保存内参到文本文件
        filename = "based on pts/camera_intrinsics.txt"
        
        with open(filename, 'w') as f:
            f.write(f"Camera Intrinsics Parameters\n")
            f.write("=" * 30 + "\n")
            f.write(f"Focal Length: {FOCAL_LENGTH}mm\n")
            f.write(f"Pixel Size: {PIXEL_SIZE}mm\n")
            f.write(f"Calibration Points: {NUM_CALIBRATION_POINTS}\n")
            f.write("-" * 30 + "\n")
            f.write("True Values:\n")
            f.write(f"fx_true: {last_true_values['camera_matrix'][0,0]:.6f}\n")
            f.write(f"fy_true: {last_true_values['camera_matrix'][1,1]:.6f}\n")
            f.write(f"cx_true: {last_true_values['camera_matrix'][0,2]:.6f}\n")
            f.write(f"cy_true: {last_true_values['camera_matrix'][1,2]:.6f}\n")
            f.write("-" * 30 + "\n")
            f.write("Calibrated Values:\n")
            f.write(f"fx: {last_successful_results['camera_matrix'][0,0]:.6f}\n")
            f.write(f"fy: {last_successful_results['camera_matrix'][1,1]:.6f}\n")
            f.write(f"cx: {last_successful_results['camera_matrix'][0,2]:.6f}\n")
            f.write(f"cy: {last_successful_results['camera_matrix'][1,2]:.6f}\n")
            f.write("-" * 30 + "\n")
            f.write("Error Statistics:\n")
            f.write(f"fx error - Mean: {np.mean(all_fx_errors):.6f}, Std: {np.std(all_fx_errors):.6f}\n")
            f.write(f"fy error - Mean: {np.mean(all_fy_errors):.6f}, Std: {np.std(all_fy_errors):.6f}\n")
            f.write(f"cx error - Mean: {np.mean(all_cx_errors):.6f}, Std: {np.std(all_cx_errors):.6f}\n")
            f.write(f"cy error - Mean: {np.mean(all_cy_errors):.6f}, Std: {np.std(all_cy_errors):.6f}\n")
        
        print(f"\n内参已保存到文件: {filename}")
    else:
        print("没有成功的标定结果可供分析")